/*
 * Utils.cpp
 *
 */

#include "../include/Utils.h"
#include "../include/Config.h"

namespace utils {

poi::POI* Utils::KeyPoint2POIfromDepthMat(const cv::KeyPoint &keypoint, const ObjectType &label, const cv::Mat &depth) {
	assert(!depth.empty());

	float dist = Utils::calcDistanceFromDepthMat(depth, keypoint.pt);
	if (dist <= 0)
		dist = -1.0f;

	float angle = Utils::calcAngleJack(keypoint.pt);

	return new poi::POI(keypoint.pt, label, dist, angle);
}

void Utils::imageCutter(cv::Mat src, const cv::Rect &rect, cv::Mat &dest) {
	assert(!src.empty());
	assert(!dest.empty());

	dest = src(rect);
}

float Utils::getPOIX(const poi::POI* point) {
	float _x = 0.0;

	float _dist = point->getDistance()*1000;
	float _angle = point->getAngle();

	_x=_dist*abs(cos(_angle));

	return _x;
}

float Utils::getPOIY(const poi::POI* point) {
	float _y = 0.0;

	float _dist = point->getDistance()*1000;
	float _angle = point->getAngle();

	_y=_dist*(-sin(_angle));

	return _y;
}


void Utils::showPOIs(cv::Mat &bgr, const std::vector<poi::POI*> pois) {
	for (unsigned int i = 0; i < pois.size(); i++) {

		cv::Point2i point = pois[i]->getPoint();
		cv::Scalar colors;

		if (pois[i]->getLabel() == OBSTACLE)
			colors = cv::Scalar(0, 0, 0, 0);
		else if (pois[i]->getLabel() == BALL)
			colors = cv::Scalar(255, 0, 0, 0);
		else if (pois[i]->getLabel() == GOAL)
			colors = cv::Scalar(0, 0, 255, 0);

		cv::circle(bgr, point, 3, colors, 3);
	}
}

} /* namespace utils */
